﻿// 2020/7/1: 首个版本
// 2020/7/2: 支持车道线指标计算
// 2020/9/29: 修正TTLC计算
// 2021/7/7: 新增laneSensorSampleProtocols
// 2023/10/11: 补充部分英文注释
// 2025/4/30: 增加laneSensorSampleConverters

var LineDescription = {
    Model : 1, // Describe with model coefficients and consistent properties / 通过模型参数和一致的属性进行描述
    Segments : 2, // Describe with many segments and properties of each segment / 通过多段线段和各自的属性进行描述
    Both : 3, // Both model and segments are supported / Model和Segments双模式描述
};

var LineClass = {
    General : 1, // 未指定
    Dashed : 2, // 虚线
    Solid : 3, // 实线
    DoubleDashed : 4, //双虚线
    DoubleSolid : 5, //双实线
    SolidDashed : 6, // 左实右虚
    DashedSolid : 7, // 左虚右实
    Curb : 8, // 路沿
    Guardrail : 9, // 护栏
    Barrier : 10, //障碍物边界
};

var LineColor = {
    Unknown : 1, // 未知
    White : 2, // 白色
    Yellow : 3, // 黄色
    Red : 4, // 红色
    Blue : 5, // 蓝色
    Orange : 6, // 橙色
    Green : 7, // 绿色
};

function LineSegmentSolidProperty() {
    this.valid = false; // Whether it's valid, false indicates virtual part / 是否有效，false表示为虚线段
    this.color = LineColor.Unknown; // Color of the line segment / 线段颜色
    this.width = null; // [m] Width of the line segment / 线段宽度
}

function LineSegment() {
    this.id = 0; // ID of line segment / 线段ID
    this.p1x = 0.0; // [m] x coordination of the start point / 线段首端点的x坐标
    this.p1y = 0.0; // [m] y coordination of the start point / 线段首端点的y坐标
    this.p1z = null; // [m] z coordination of the start point / 线段首端点的z坐标
    this.p2x = 0.0; // [m] x coordination of the end point / 线段末端点的x坐标
    this.p2y = 0.0; // [m] y coordination of the end point / 线段末端点的y坐标
    this.p2z = null; // [m] z coordination of the end point / 线段末端点的z坐标
    this.solidProperty = new LineSegmentSolidProperty(); // Properties of solid line segment (Not for virtual part) / 实线段的属性（虚线段则为空）
}

function LaneLine() {
    this.id = 0; // ID of the lane line, zero as invalid / 车道线ID，0表示无效
    this.rawID = null; // Raw ID of the lane line / 车道线原始ID
    this.confidence = null; // [%] Confidence of the lane line / 车道线置信度
    this.description = LineDescription.Model; // How to describe the lane line's curve / 车道线的描述方式
    this.classification = LineClass.General; // Model description: Classification of lane line / Model模式：一致性车道线类型
    this.color = LineColor.Unknown; // Model description: Color of lane line / Model模式：一致性车道线颜色
    this.width = null; // [m] Model description: Line width of lane line / Model模式：一致性车道线宽度
    this.param0 = 0.0; // Model description: Zero order coefficient of y-function of lane line / Model模式：车道线y方程0次方系数
    this.param1 = 0.0; // Model description: First order coefficient of y-function of lane line / Model模式：车道线y方程1次方系数
    this.param2 = 0.0; // Model description: Second order coefficient of y-function of lane line / Model模式：车道线y方程2次方系数
    this.param3 = 0.0; // Model description: Third order coefficient of y-function of lane line / Model模式：车道线y方程3次方系数
    this.paramzValid = false; // Model description: Whether z-function is valid / Model模式：z方程是否有效
    this.paramz0 = 0.0; // Model description: Zero order coefficient of z-function of lane line / Model模式：车道线z方程0次方系数
    this.paramz1 = 0.0; // Model description: First order coefficient of z-function of lane line / Model模式：车道线z方程1次方系数
    this.paramz2 = 0.0; // Model description: Second order coefficient of z-function of lane line / Model模式：车道线z方程2次方系数
    this.paramz3 = 0.0; // Model description: Third order coefficient of z-function of lane line / Model模式：车道线z方程3次方系数
    this.rearEnd = null; // [m] Model description: X-axis coordination of read end of the lane line / Model模式：曲线后端点位置(x轴)
    this.frontEnd = null; // [m] Model description: X-axis coordination of front end of the lane line / Model模式：曲线前端点位置(x轴)
    this.segments = []; // Segment description: List of line segments / Segments模式：线段列表
    this.modelValid = function () { // Whether model description is supported / 是否支持Model模式
        return this.description == LineDescription.Model || this.description == LineDescription.Both;
    }
    this.segmentsValid = function () { // Whether segment description is supported / 是否支持Segments模式
        return this.description == LineDescription.Segments || this.description == LineDescription.Both;
    }
}

function LaneSensorSample(session, time) {
    this.session = session; // Date or AppoxSessionIdentifier
    this.time = time; // [s] Time offset in session / Session内的相对时间
    this.lines = []; // List of lane lines / 车道线列表
    this.firstLeftIndex = null; // Index of the first lane line to the left side, -1 indicated not exist / 左侧第一条车道线序号，-1表示不存在
    this.firstRightIndex = null; // Index of the first lane line to the right side, -1 indicated not exist / 右侧第一条车道线序号，-1表示不存在
    this.secondLeftIndex = null; // Index of the second lane line to the left side, -1 indicated not exist / 左侧第二条车道线序号，-1表示不存在
    this.secondRightIndex = null; // Index of the second lane line to the right side, -1 indicated not exist / 右侧第二条车道线序号，-1表示不存在
    this.laneWidth = null; // [m] Current lane's width / 当前车道宽度
    this.laneHeading = null; // [deg] Current lane's orientation / 当前车道朝向
    this.laneCurvature = null; // [1/m] Current lane's curvature / 当前车道曲率
    this.lateralVelocity = null; // [m/s] Lateral speed to current lane / 当前车道横向速度
    this.centerDeparture = null; // [m] Central departure to current lane / 当前车道中央偏离距离
    this.vehicleSpeed = null; // [kph] Subject vehicle's speed / 本车车速
    this.vehicleCurvature = null; // [1/m] Subject vehicle's turning curvature / 本车行驶曲率
    this.vehicleAX = null; // [m/s²] Subject vehicle's longitudinal acceleration / 本车纵向加速度
    this.vehicleWidth = null; // [m] Subject vehicle's width / 本车宽度
    this.vehicleFrontOverhang = null; // [m] Subject vehicle's front overhang / 本车前悬
    this.vehicleWheelBase = null; // [m] Subject vehicle's wheel base / 本车轴距
    this.rearBound = 0.0; // [m] Rear bound of detect range / 传感器检测范围后边界
    this.frontBound = 30.0; // [m] Front bound of detect range / 传感器检测范围前边界
    this.getDTLC = function (line) { // Calculate DTLC / 计算DTLC [m]
        if (line.modelValid() && this.vehicleWidth != null && this.vehicleFrontOverhang != null) {
            var isLeft = line.param0 > 0;
            var x = -1 * this.vehicleFrontOverhang;
            var y = (line.param3 * x * x * x + line.param2 * x * x
                + line.param1 * x + line.param0) * (isLeft ? 1 : -1);
            var cosTheta = Math.abs(Math.cos(Math.atan(line.param1)));
            return cosTheta * (y - this.vehicleWidth / 2.0);
        }
        else return null;
    }
    this.getSTLC = function (line) { // Calculate STLC / 计算STLC [m/s]
        if (line.modelValid() && this.vehicleSpeed != null) {
            var isLeft = line.param0 > 0;
            var curv = this.vehicleCurvature == null ? 0 : this.vehicleCurvature;
            var fo = this.vehicleFrontOverhang == null ? 0.9 : this.vehicleFrontOverhang;
            var wb = this.vehicleWheelBase == null ? 2.8 : this.vehicleWheelBase;
            var sinTheta = Math.sin(Math.atan(line.param1 - curv * (fo + wb)));
            return (isLeft ? -1.0 : 1.0) * sinTheta * this.vehicleSpeed / 3.6;
        }
        else return null;
    }
    this.getTTLC = function (line) { // Calculate TTLC / 计算TTLC [s]
        if (line.modelValid() && this.vehicleSpeed != null && this.vehicleWidth != null && this.vehicleFrontOverhang != null) {
            if (this.vehicleSpeed <= 0) return null;
            var isLeft = line.param0 > 0;
            var wb = this.vehicleWheelBase != null ? this.vehicleWheelBase : 2.8;
            var x0 = -this.vehicleFrontOverhang - wb;
            var x = x0;
            var y = 0;
            var vx = this.vehicleSpeed / 3.6;
            var loopTimes = vx < 5 ? 1000 : Math.floor(50.0 / vx * 100); // No output if crossing beyond 50m (Can't guarantee precision) / 超过50米越线则不输出（精度无法保证）
            var rx = wb;
            var ry = (isLeft ? 0.5 : -0.5) * this.vehicleWidth;
            for (var i = 0; i <= loopTimes; i++) {
                var k = 0;
                if (this.vehicleCurvature != null) k = this.vehicleCurvature * (x - x0);
                var cosa = 1.0 / Math.abs(1 + k);
                var sina = cosa * k;
                var tx = x + cosa * rx - sina * ry;
                var ty = y + sina * rx + cosa * ry;
                var ly = line.param3 * tx * tx * tx + line.param2 * tx * tx + line.param1 * tx + line.param0;
                var passed = false;
                if (isLeft) {
                    if (ly <= ty) passed = true;
                }
                else {
                    if (ly >= ty) passed = true;
                }
                if (passed) {
                    if (i == 0) return null;
                    else return i / 100;
                }
                if (this.vehicleAX != null) vx += this.vehicleAX * 0.01;
                if (vx <= 0) break;
                x += vx * 0.01 * cosa;
                y += vx * 0.01 * sina;
            }
            return null;
        }
        else return null;
    }
}

function convLaneSensorSampleV3(gs) {
    if (gs.values.length < 17) return null;

    var lineCount = gs.values[0] == null ? 0 : gs.values[0];
    if (gs.values.length != 17 + lineCount * 15) return null;

    var sample = new LaneSensorSample(gs.session, gs.time);
    sample.lateralVelocity = gs.values[1];
    sample.centerDeparture = gs.values[2];
    sample.laneWidth = gs.values[3];
    sample.laneHeading = gs.values[4];
    sample.laneCurvature = gs.values[5];
    sample.vehicleSpeed = gs.values[6];
    sample.vehicleCurvature = gs.values[7];
    sample.vehicleAX = gs.values[8];
    sample.vehicleWidth = gs.values[9];
    sample.vehicleFrontOverhang = gs.values[10];
    sample.rearBound = gs.values[11];
    sample.frontBound = gs.values[12];
    sample.firstLeftIndex = gs.values[13];
    sample.firstRightIndex = gs.values[14];
    sample.secondLeftIndex = gs.values[15];
    sample.secondRightIndex = gs.values[16];

    sample.lines = Array(lineCount);
    for (var i = 0; i < lineCount; i++) {
        var line = new LaneLine();
        var baseIndex = 17 + 15 * i;
        line.description = LineDescription.Model;
        if (gs.values[baseIndex] != null) line.id = gs.values[baseIndex];
        line.rawID = gs.values[baseIndex + 1];
        if (gs.values[baseIndex + 2] != null) line.classification = gs.values[baseIndex + 2];
        if (gs.values[baseIndex + 3] != null) line.color = gs.values[baseIndex + 3];
        line.confidence = gs.values[baseIndex + 4];
        line.width = gs.values[baseIndex + 5];
        if (gs.values[baseIndex + 6] != null) line.param0 = gs.values[baseIndex + 6];
        if (gs.values[baseIndex + 7] != null) line.param1 = gs.values[baseIndex + 7];
        if (gs.values[baseIndex + 8] != null) line.param2 = gs.values[baseIndex + 8];
        if (gs.values[baseIndex + 9] != null) line.param3 = gs.values[baseIndex + 9];
        line.rearEnd = gs.values[baseIndex + 10];
        line.frontEnd = gs.values[baseIndex + 11];
        sample.lines[i] = line;
    }

    return sample;
}

function convLaneSensorSampleV4(gs) {
    if (gs.values.length < 23) return null;

    var lineCount = gs.values[0] == null ? 0 : gs.values[0];
    var segmentCount = gs.values[1] == null ? 0 : gs.values[1];
    if (gs.values.length != 23 + lineCount * 27 + segmentCount * 12) return null;

    var sample = new LaneSensorSample(gs.session, gs.time);
    sample.lateralVelocity = gs.values[2];
    sample.centerDeparture = gs.values[3];
    sample.laneWidth = gs.values[4];
    sample.laneHeading = gs.values[5];
    sample.laneCurvature = gs.values[6];
    sample.rearBound = gs.values[7];
    sample.frontBound = gs.values[8];
    sample.firstLeftIndex = gs.values[9];
    sample.firstRightIndex = gs.values[10];
    sample.secondLeftIndex = gs.values[11];
    sample.secondRightIndex = gs.values[12];
    sample.vehicleSpeed = gs.values[13];
    sample.vehicleCurvature = gs.values[14];
    sample.vehicleAX = gs.values[15];
    sample.vehicleWidth = gs.values[16];
    sample.vehicleFrontOverhang = gs.values[17];
    sample.vehicleWheelBase = gs.values[18];

    var segBase = 23 + lineCount * 27;

    sample.lines = Array(lineCount);
    for (var i = 0; i < lineCount; i++) {
        var line = new LaneLine();
        var baseIndex = 23 + 27 * i;

        if (gs.values[baseIndex] != null) line.id = gs.values[baseIndex];
        line.rawID = gs.values[baseIndex + 1];
        line.confidence = gs.values[baseIndex + 2];
        if (gs.values[baseIndex + 3] != null) line.description = gs.values[baseIndex + 3];

        if (line.modelValid()) {
            if (gs.values[baseIndex + 4] != null) line.classification = gs.values[baseIndex + 4];
            if (gs.values[baseIndex + 5] != null) line.color = gs.values[baseIndex + 5];
            line.width = gs.values[baseIndex + 6];
            if (gs.values[baseIndex + 7] != null) line.param0 = gs.values[baseIndex + 7];
            if (gs.values[baseIndex + 8] != null) line.param1 = gs.values[baseIndex + 8];
            if (gs.values[baseIndex + 9] != null) line.param2 = gs.values[baseIndex + 9];
            if (gs.values[baseIndex + 10] != null) line.param3 = gs.values[baseIndex + 10];
            if (gs.values[baseIndex + 11] == 1) {
                line.paramzValid = true;
                if (gs.values[baseIndex + 12] != null) line.paramz0 = gs.values[baseIndex + 12];
                if (gs.values[baseIndex + 13] != null) line.paramz1 = gs.values[baseIndex + 13];
                if (gs.values[baseIndex + 14] != null) line.paramz2 = gs.values[baseIndex + 14];
                if (gs.values[baseIndex + 15] != null) line.paramz3 = gs.values[baseIndex + 15];
            }
            line.rearEnd = gs.values[baseIndex + 16];
            line.frontEnd = gs.values[baseIndex + 17];
        }

        if (line.segmentsValid()) {
            if (gs.values[baseIndex + 25] != null && gs.values[baseIndex + 26] != null) {
                var segOffset = gs.values[baseIndex + 25];
                var segSize = gs.values[baseIndex + 26];
                if (segSize > 0) {
                    for (var n = 0; n < segSize; n++) {
                        var seg = new LineSegment();
                        var segBaseIndex = segBase + 12 * (segOffset + n);
                        if (gs.values[segBaseIndex] == null ||
                            gs.values[segBaseIndex + 1] == null ||
                            gs.values[segBaseIndex + 2] == null ||
                            gs.values[segBaseIndex + 4] == null ||
                            gs.values[segBaseIndex + 5] == null) continue;
                        seg.id = gs.values[segBaseIndex];
                        seg.p1x = gs.values[segBaseIndex + 1];
                        seg.p1y = gs.values[segBaseIndex + 2];
                        seg.p1z = gs.values[segBaseIndex + 3];
                        seg.p2x = gs.values[segBaseIndex + 4];
                        seg.p2y = gs.values[segBaseIndex + 5];
                        seg.p2z = gs.values[segBaseIndex + 6];
                        if (gs.values[segBaseIndex + 7] == 1) {
                            seg.solidProperty.valid = true;
                            if (gs.values[segBaseIndex + 8] != null) seg.solidProperty.color = gs.values[segBaseIndex + 8];
                            seg.solidProperty.width = gs.values[segBaseIndex + 9];
                        }
                        line.segments.push(seg);
                    }
                }
            }
        }

        sample.lines[i] = line;
    }

    return sample;
}

// Interpolator / 样本插值函数
function interpolateLaneSensorSample(session, time, s1, w1, s2, w2) {
    var sample = new LaneSensorSample(session, time);
    sample.lateralVelocity = s1.lateralVelocity == null || s2.lateralVelocity == null ? null : s1.lateralVelocity * w1 + s2.lateralVelocity * w2;
    sample.centerDeparture = s1.centerDeparture == null || s2.centerDeparture == null ? null : s1.centerDeparture * w1 + s2.centerDeparture * w2;
    sample.laneWidth = s1.laneWidth == null || s2.laneWidth == null ? null : s1.laneWidth * w1 + s2.laneWidth * w2;
    sample.laneCurvature = s1.laneCurvature == null || s2.laneCurvature == null ? null : s1.laneCurvature * w1 + s2.laneCurvature * w2;
    sample.laneHeading = s1.laneHeading == null || s2.laneHeading == null ? null : s1.laneHeading * w1 + s2.laneHeading * w2;
    sample.vehicleSpeed = s1.vehicleSpeed == null || s2.vehicleSpeed == null ? null : s1.vehicleSpeed * w1 + s2.vehicleSpeed * w2;
    sample.vehicleCurvature = s1.vehicleCurvature == null || s2.vehicleCurvature == null ? null : s1.vehicleCurvature * w1 + s2.vehicleCurvature * w2;
    sample.vehicleAX = s1.vehicleAX == null || s2.vehicleAX == null ? null : s1.vehicleAX * w1 + s2.vehicleAX * w2;
    sample.vehicleWidth = s1.vehicleWidth;
    sample.vehicleFrontOverhang = s1.vehicleFrontOverhang;
    sample.vehicleWheelBase = s1.vehicleWheelBase;
    sample.rearBound = w1 > w2 ? s1.rearBound : s2.rearBound;
    sample.frontBound = w1 > w2 ? s1.frontBound : s2.frontBound;

    var firstLeftID = null, firstRightID = null, secondLeftID = null, secondRightID = null;
    if (s1.firstLeftIndex != null && s2.firstLeftIndex != null && s1.lines[s1.firstLeftIndex].id == s2.lines[s2.firstLeftIndex].id) {
        firstLeftID = s1.lines[s1.firstLeftIndex].id;
    }
    if (s1.firstRightIndex != null && s2.firstRightIndex != null && s1.lines[s1.firstRightIndex].id == s2.lines[s2.firstRightIndex].id) {
        firstRightID = s1.lines[s1.firstRightIndex].id;
    }
    if (s1.secondLeftIndex != null && s2.secondLeftIndex != null && s1.lines[s1.secondLeftIndex].id == s2.lines[s2.secondLeftIndex].id) {
        secondLeftID = s1.lines[s1.secondLeftIndex].id;
    }
    if (s1.secondRightIndex != null && s2.secondRightIndex != null && s1.lines[s1.secondRightIndex].id == s2.lines[s2.secondRightIndex].id) {
        secondRightID = s1.lines[s1.secondRightIndex].id;
    }

    var index = 0;
    for (var i1 = 0; i1 < s1.lines.length; i1++) {
        var o1 = s1.lines[i1];
        var id = o1.id;
        for (var i2 = 0; i2 < s2.lines.length; i2++) {
            var o2 = s2.lines[i2];
            if (o2.id != id) continue;
            if (firstLeftID != null && firstLeftID == id) sample.firstLeftIndex = index;
            if (firstRightID != null && firstRightID == id) sample.firstRightIndex = index;
            if (secondLeftID != null && secondLeftID == id) sample.secondLeftIndex = index;
            if (secondRightID != null && secondRightID == id) sample.secondRightIndex = index;

            var line = new LaneLine();
            line.id = id;
            line.rawID = o1.rawID;
            line.confidence = o1.confidence == null || o2.confidence == null ? null : o1.confidence * w1 + o2.confidence * w2;
            line.description = o1.description;

            if (line.modelValid()) {
                line.classification = w1 > w2 ? o1.classification : o2.classification;
                line.color = w1 > w2 ? o1.color : o2.color;
                line.width = o1.width == null || o2.width == null ? null : o1.width * w1 + o2.width * w2;
                line.param0 = o1.param0 * w1 + o2.param0 * w2;
                line.param1 = o1.param1 * w1 + o2.param1 * w2;
                line.param2 = o1.param2 * w1 + o2.param2 * w2;
                line.param3 = o1.param3 * w1 + o2.param3 * w2;
                if (o1.paramzValid && o2.paramzValid) {
                    line.paramzValid = true;
                    line.paramz0 = o1.paramz0 * w1 + o2.paramz0 * w2;
                    line.paramz1 = o1.paramz1 * w1 + o2.paramz1 * w2;
                    line.paramz2 = o1.paramz2 * w1 + o2.paramz2 * w2;
                    line.paramz3 = o1.paramz3 * w1 + o2.paramz3 * w2;
                }
                line.rearEnd = o1.rearEnd == null || o2.rearEnd == null ? null : o1.rearEnd * w1 + o2.rearEnd * w2;
                line.frontEnd = o1.frontEnd == null || o2.frontEnd == null ? null : o1.frontEnd * w1 + o2.frontEnd * w2;
            }

            if (line.segmentsValid()) {
                for (var si1 = 0; si1 < o1.segments.length; si1++) {
                    var seg1 = o1.segments[si1];
                    for (var si2 = 0; si2 < o2.segments.length; si2++) {
                        var seg2 = o2.segments[si2];
                        if (seg1.id == seg2.id) {
                            var seg = new LineSegment();
                            seg.id = seg1.id;
                            seg.p1x = seg1.p1x * w1 + seg2.p1x * w2;
                            seg.p1y = seg1.p1y * w1 + seg2.p1y * w2;
                            seg.p2x = seg1.p2x * w1 + seg2.p2x * w2;
                            seg.p2y = seg1.p2y * w1 + seg2.p2y * w2;
                            seg.p1z = seg1.p1z == null || seg2.p1z == null ? null : seg1.p1z * w1 + seg2.p1z * w2;
                            seg.p2z = seg1.p2z == null || seg2.p2z == null ? null : seg1.p2z * w1 + seg2.p2z * w2;
                            seg.solidProperty = w1 > w2 ? seg1.solidProperty : seg2.solidProperty;
                            line.segments.push(seg);
                        }
                    }
                }
            }

            sample.lines.push(line);
            index++;
        }
    }

    return sample;
}

// Supported protocols / 车道线传感器样本支持解析的协议
var laneSensorSampleProtocols = [
    "lane-sensor-sample-v3",
    "lane-sensor-sample-v4",
];

// Converters / 通用样本转换函数表
var laneSensorSampleConverters = {
    "lane-sensor-sample-v3": convLaneSensorSampleV3,
    "lane-sensor-sample-v4": convLaneSensorSampleV4,
};

// Use this function to query sample at target time. If "interpolated" is true, the interpolated frame is preferred, otherwise the nearest frame / 获取指定session指定时刻的车道线传感器样本，若interpolated为true则优先取插值帧，否则固定为最近帧
function getLaneSensorSample(channel, session, time, interpolated) {
    var s1 = null, w1 = null, s2 = null, w2 = null;
    var pairV4 = biGetGeneralSamplePair('lane-sensor-sample-v4', channel, session, time, 1);
    var pairV3 = biGetGeneralSamplePair('lane-sensor-sample-v3', channel, session, time, 1);
    if (pairV4 != null) {
        if (pairV4.sample1 != null) s1 = convLaneSensorSampleV4(pairV4.sample1);
        if (pairV4.sample2 != null) s2 = convLaneSensorSampleV4(pairV4.sample2);
        w1 = pairV4.weight1;
        w2 = pairV4.weight2;
    }
    else if (pairV3 != null) {
        if (pairV3.sample1 != null) s1 = convLaneSensorSampleV3(pairV3.sample1);
        if (pairV3.sample2 != null) s2 = convLaneSensorSampleV3(pairV3.sample2);
        w1 = pairV3.weight1;
        w2 = pairV3.weight2;
    }
    else return null;
    if (s1 == null || s2 == null) {
        return s1 == null ? s2 : s1;
    }
    if (interpolated) {
        return interpolateLaneSensorSample(session, time, s1, w1, s2, w2);
    }
    else {
        return w1 > w2 ? s1 : s2;
    }
}